#include "GPPhysicsSystem.h"
#include "GPRegisterThreads.h"
#include "GPTimings.h"

#include <fstream>
#include <iostream>
#include <string>
#include <sstream>

const int MAX_CONTACTS = 6;

/*!
 * \brief This is the callback function given to ODE to handle the collision between two objects.
 * 
 * \param data 
 * The pointer to the physics system object
 * 
 * \param o0
 * The first object
 * 
 * \param o1
 * The second object
 */
void nearCallback(void *data, dGeomID o0, dGeomID o1) {
	reinterpret_cast<GPPhysicsSystem*>(data)->handleCollisionBetween(o0,o1);
} 

/*!
 * \brief The constructor of the class.
 * 
 * \param stepsize The stepsize for the physics iterations.
 * \param requireRegister A boolean describing whether or not the thread should registered with Ogre
 * 
 * This constructor create both the simulation ODE world and the ODE space. The space
 *  that is created is a ODE HashSpace. The gravity is also set in the world. This pulls
 *  all the objects into the direction of the negative y-axis.
 */
GPPhysicsSystem::GPPhysicsSystem(float stepsize, bool requireRegister) : GPThreadedSystem(requireRegister), stepsize_(stepsize) {
	world_           = dWorldCreate();
	dWorldSetGravity (world_,0,-9.81f,0);

	space_           = dHashSpaceCreate(0);
	contacts_        = dJointGroupCreate(0); 
	nextGeometryID_  = 0;

	contact_ = new dContact[MAX_CONTACTS];
    for (int i = 0; i < MAX_CONTACTS; i++) {
      contact_[i].surface.mode       = dContactSoftCFM | dContactBounce ;
      contact_[i].surface.mu         = dInfinity;
      contact_[i].surface.mu2        = 0.0f;
      contact_[i].surface.bounce     = 0.0f;
      contact_[i].surface.bounce_vel = 0.01f;
      contact_[i].surface.soft_cfm   = 0.05f;
	  contact_[i].surface.slip1      = 0.0f;
	  contact_[i].surface.slip2      = 0.0f;
    }
}

/*!
 * \brief The destructor.
 * 
 * This destructor destroys the ODE world and the ODE space.
 */
GPPhysicsSystem::~GPPhysicsSystem() {
	dWorldDestroy(world_);
	dSpaceDestroy(space_);
	dJointGroupDestroy(contacts_);
}

/*!
 * \brief The actual function that is called when running the thread.
 * 
 * This is the real function of the physics system. Here everything is started
 * and update while the system doesn't have to stop.
 */
void GPPhysicsSystem::run() {
	// register the thread with Ogre
	doRegisterThread();

	// while the system is not initialized, sleep!
	while (!initialized_) microSleep(1000);

	// while the system doesn't have to stop, perform an iteration
	t_=0;
	lastsimulationtime_ = getTime();
	while (!hasToStop_)
		update();
}

/*!
 * \brief This creates the ground plane.
 * 
 * \returns The id of the object created in the function.
 */
int GPPhysicsSystem::createGroundPlane() {
	dGeomID geomid  = dCreatePlane(space_, 0, 1, 0, 0);
	int id =  ++nextGeometryID_;
	geometryIDs_[id]=geomid;
	return id;
}

/*!
 * \brief Creates a box in the physics system.
 * 
 * \param w The width of the box
 * \param h The height of the box
 * \param d The depth of the box
 * \param x The x position of the box
 * \param y The y position of the box
 * \param z The z position of the box
 * \param qx The x value of the quaternion for the orientation
 * \param qy The y value of the quaternion for the orientation
 * \param qz The z value of the quaternion for the orientation
 * \param qw The w value of the quaternion for the orientation
 * \param vx The x component of the linear velocity
 * \param vy The y component of the linear velocity
 * \param vz The z component of the linear velocity
 * \param wx The x component of the angular velocity
 * \param wy The y component of the angular velocity
 * \param wz The z component of the angular velocity
 * \param mass The mass of the box
 * \param dynamic Indicates whether the box can move. If true, the box can move due to physics.
 * 
 * \returns The id of the box created.
 */
int GPPhysicsSystem::createBox(float w, float h, float d,
							   float x, float y, float z,
							   float qx, float qy, float qz, float qw,
							   float vx, float vy, float vz,
							   float wx, float wy, float wz, 
							   float mass, float dynamic) 
{
	// first lock the physics system
	GPSYNCHRONIZED;
	dMass odeMass;
	dQuaternion quat;

	// create a new id
	int id = ++nextGeometryID_;

	// create the ODE geometry in the ODE space
	dGeomID geom = dCreateBox(space_,w,h,d);

	if (dynamic) {
		// if the box can move due to physics, create the body in the ODE world
		dBodyID body = dBodyCreate(world_);

		// connect the body to the geometry
		dGeomSetBody(geom,body);

		float density = mass / (w*h*d);

		// set the mass properties
		dMassSetBox(&odeMass,density,w,h,d);
		// set the mass
		dBodySetMass(body,&odeMass);
		// set the linear velocity
		dBodySetLinearVel(body, vx, vy, vz);
		// set the angular velocity
		dBodySetAngularVel(body, wx, wy, wz);

		// store the body
		bodyIDs_[id] = body;
	}

	// set the position of the geometry
	dGeomSetPosition(geom, x, y, z);

	// set the orientation of the geometry
	quat[0] = qw; quat[1] = qx; quat[2] = qy; quat[3] = qz;
	dGeomSetQuaternion(geom, quat);

	// store the geometry
	geometryIDs_[id] = geom;

	// return the new id
	return id;
}

/*!
 * \brief Creates a sphere in the physics system.
 * 
 * \param r The radius of the sphere
 * \param x The x position of the radius
 * \param y The y position of the radius
 * \param z The z position of the radius
 * \param qx The x value of the quaternion for the orientation
 * \param qy The y value of the quaternion for the orientation
 * \param qz The z value of the quaternion for the orientation
 * \param qw The w value of the quaternion for the orientation
 * \param vx The x component of the linear velocity
 * \param vy The y component of the linear velocity
 * \param vz The z component of the linear velocity
 * \param wx The x component of the angular velocity
 * \param wy The y component of the angular velocity
 * \param wz The z component of the angular velocity
 * \param mass The mass of the radius
 * \param dynamic Indicates whether the radius can move. If true, the radius can move due to physics.
 * 
 * \returns The id of the radius created.
 */
int GPPhysicsSystem::createSphere(float r,
							   float x, float y, float z,
							   float qx, float qy, float qz, float qw,
							   float vx, float vy, float vz,
							   float wx, float wy, float wz, 
							   float mass, float dynamic) 
{
	// lock the physics system
	GPSYNCHRONIZED;
	dMass odeMass;
	dQuaternion quat;

	// create a new id
	int id = ++nextGeometryID_;

	// create the sphere geometry in the ODE space
	dGeomID geom = dCreateSphere(space_,r);

	if (dynamic) {
		// if the sphere should move, create the ODE body in the ODE world
		dBodyID body = dBodyCreate(world_);

		// connect the geometry and body
		dGeomSetBody(geom,body);

		float density = mass / (4.0f * M_PI * pow(r,3.f) / 3.0f);
		// set the mass property
		dMassSetSphere(&odeMass,density,r);
		// set the mass
		dBodySetMass(body,&odeMass);
		// set the linear velocity
		dBodySetLinearVel(body, vx, vy, vz);
		// set the angular velocity
		dBodySetAngularVel(body, wx, wy, wz);

		// store the body
		bodyIDs_[id] = body;
	}

	// set the position of the geometry
	dGeomSetPosition(geom, x, y, z);

	// set the orientation of the geometry
	quat[0] = qw; quat[1] = qx; quat[2] = qy; quat[3] = qz;
	dGeomSetQuaternion(geom, quat);

	// store the geometry
	geometryIDs_[id] = geom;

	// return the id
	return id;
}

/*!
 * \brief This is the function that performs one iteration of the ODE simulation
 */
void GPPhysicsSystem::update() {

	// get the current time
	walltime now = getTime();

	// calculate the seconds elapsed since the start of the physics system
	float secsElapsed = (float)diffWalltime(lastsimulationtime_, now);

	// if the current time is less than the elapsed seconds do an iteration.
	if (t_ < secsElapsed) {

		// first lock the physics system
		synchronizeEnter();

		//apply forces
		applyAdditionalForces();
		
		// Detect collision
		dSpaceCollide(space_, this, &nearCallback);
		//get accelerations at every step
		getAcc();

		// Perform an iteration
		dWorldQuickStep(world_, (dReal)stepsize_);

		dJointGroupEmpty(contacts_); 

		// unlocks the physics system
		synchronizeLeave();

		// increase the current time of the physics system
		t_+=stepsize_;
	} else {
		// otherwise sleep for 1 millisecond
		microSleep(1000);
	}
}

/*!
 * \brief This function handles the collision between two objects
 * 
 * \param o0 The first object.
 * \param o1 The second object.
 */
void GPPhysicsSystem::handleCollisionBetween(dGeomID o0, dGeomID o1) {

	if (int numc = dCollide(o0, o1, MAX_CONTACTS, &contact_[0].geom, sizeof(dContact))) {
		// Get the dynamics body for each geom
		dBodyID b1 = dGeomGetBody(o0);
		dBodyID b2 = dGeomGetBody(o1);
		// To add each contact point found to our joint group we call dJointCreateContact which is just one of the many
		// different joint types available. 
		for (int i = 0; i < numc; i++)
		{
			// dJointCreateContact needs to know which world and joint group to work with as well as the dContact
			// object itself. It returns a new dJointID which we then use with dJointAttach to finally create the
			// temporary contact joint between the two geom bodies.
			dJointID c = dJointCreateContact(world_, contacts_, contact_ + i);
			dJointAttach(c, b1, b2);
		}
	} 
}

/*!
 * \brief Get the current position of an object.
 * 
 * \param id The id of the object
 * 
 * \param x This variable will be filled with the x coordinate of the current position
 * \param y This variable will be filled with the y coordinate of the current position
 * \param z This variable will be filled with the z coordinate of the current position
 */
void GPPhysicsSystem::getPosition(int id, float& x, float& y, float& z) {
	dGeomID geomID = geometryIDs_[id];
	const dReal* pos = dGeomGetPosition(geomID);
	x = pos[0];
	y = pos[1];
	z = pos[2];
}

/*!
 * \brief Get the mass of an object.
 * 
 * \param id The id of the object
 * 
 * \param mass This variable will be filled with the mass
 */
void GPPhysicsSystem::getMass(int id, float& mass) {
	dMass m;
	if (bodyIDs_.find(id)!=bodyIDs_.end()) {
		dBodyGetMass(bodyIDs_[id],&m);
		mass = m.mass;
	} else 
		mass = 0;
}

/*!
 * \brief Get the radius of a sphere.
 * 
 * \param id The id of the object
 * 
 * \param r This variable will be filled with the radius
 */
void GPPhysicsSystem::getSize(int id, float& r) {
	if (geometryIDs_.find(id)!=geometryIDs_.end()) {
		r = dGeomSphereGetRadius(geometryIDs_[id]);
	} else 
		r = 0;
}

/*!
 * \brief Get the size of a box.
 * 
 * \param id The id of the object
 * 
 * \param w This variable will be filled with the width of the box
 * \param h This variable will be filled with the height of the box
 * \param d This variable will be filled with the depth of the box
 */
void GPPhysicsSystem::getSize(int id, float& w, float& h, float& d) {
	if (geometryIDs_.find(id)!=geometryIDs_.end()) {
		dVector3 result;
		dGeomBoxGetLengths(geometryIDs_[id], result);
		w = result[0];
		h = result[2];
		d = result[1];
	} else {
		w = 0;
		h = 0;
		d = 0;
	}
}


/*!
 * \brief Get the current orientation of an object.
 * 
 * \param id The id of the object
 * 
 * \param x This variable will be filled with the x value of the quaternion specifying the current orientation
 * \param y This variable will be filled with the y value of the quaternion specifying the current orientation
 * \param z This variable will be filled with the z value of the quaternion specifying the current orientation
 * \param w This variable will be filled with the w value of the quaternion specifying the current orientation
 */
void GPPhysicsSystem::getOrientation(int id, float& x, float& y, float& z, float& w) {
	dGeomID geomID = geometryIDs_[id];
	dQuaternion result;
	dGeomGetQuaternion(geomID, result);
	w = result[0];
	x = result[1];
	y = result[2];
	z = result[3];
}
void GPPhysicsSystem::getAcceleration(int id, float& ax, float& ay, float& az){
	if(accelerations_.find(id)!=accelerations_.end())
	{
		float* temp = accelerations_[id];
		ax = temp[0];
		ay = temp[1];
		az = temp[2];
	}
	else{
		ax =0 ;
		ay=-10 ; 
		az=-0;
	}
}
void GPPhysicsSystem::getAcc(){
	dMass mass;
	const dReal *force;
	
	std::map<int, dBodyID>::iterator bodyIt = bodyIDs_.begin();
	while (bodyIt != bodyIDs_.end()) {
	dBodyGetMass(bodyIt->second,&mass);
	force = dBodyGetForce(bodyIt->second);
	float temp[3];
	temp[0]=(float)force[0]/mass.mass;
	temp[1]=(float)force[1]/mass.mass;
	temp[2]=(float)force[2]/mass.mass;
	accelerations_[bodyIt->first]=temp;

		++bodyIt;
	}

}

/*!
 * \brief Get the current linear velocity of an object.
 * 
 * \param id The id of the object
 * 
 * \param vx This variable will be filled with the x value of the velocity
 * \param vy This variable will be filled with the y value of the velocity
 * \param vz This variable will be filled with the z value of the velocity
 */
void GPPhysicsSystem::getLinearVelocity(int id, float& vx, float& vy, float& vz) {
	if (bodyIDs_.find(id)!=bodyIDs_.end()) {
		const dReal* result = dBodyGetLinearVel(bodyIDs_[id]);
		vx = result[0];
		vy = result[1];
		vz = result[2];
	} else {
		vx=0;
		vy=0;
		vz=0;
	}
}

/*!
 * \brief Get the current angular velocity of an object.
 * 
 * \param id The id of the object
 * 
 * \param vx This variable will be filled with the x value of the velocity
 * \param vy This variable will be filled with the y value of the velocity
 * \param vz This variable will be filled with the z value of the velocity
 */
void GPPhysicsSystem::getAngularVelocity(int id, float& vx, float& vy, float& vz) {
	if (bodyIDs_.find(id)!=bodyIDs_.end()) {
		const dReal* result = dBodyGetAngularVel(bodyIDs_[id]);
		vx = result[0];
		vy = result[1];
		vz = result[2];
	} else {
		vx=0;
		vy=0;
		vz=0;
	}
}

/*!
 * \brief Removes an object from the physics system
 * 
 * \param index The id of the object to be removed.
 */
void GPPhysicsSystem::removeObject(int index) {
	std::map<int, dGeomID>::iterator geomit = geometryIDs_.find(index);
	std::map<int, dBodyID>::iterator bodyit = bodyIDs_.find(index);
	if (bodyit!=bodyIDs_.end()) {
		dBodyDestroy(bodyit->second);
		bodyIDs_.erase(bodyit);
	}
	if (geomit!=geometryIDs_.end()) {
		dGeomDestroy(geomit->second);
		geometryIDs_.erase(geomit);
	}
}

/*!
 * \brief This function removes all spaces at the beginning and the end of a string
 * \param value The string that should be stripped from all leading and final spaces.
 */
void trimspaces(std::string& value) {
	// set the type of characters that should be removed from the string
	char const* delims = " \t\r\n";
	// find the position of the first character that is not a delimeter 
	size_t pos = value.find_first_not_of(delims);
	// remove all characters upto the position specified
	value.erase(0,pos);
	// find the last non-delimeter in the remaining string
	pos = value.find_last_not_of(delims);
	// remove all characters after the position specified
	value.erase(pos+1);
}

/*!
 * \brief Reads a settings file.
 * 
 * \param filename The name of the file containing the settings
 * 
 * This function reads a file specified by the filename argument. The
 * file is a \<property\>=\<value\> file. If the first character of a line is
 * a \#, the line is considered a comment line.
 */
void GPPhysicsSystem::loadSettings(const std::string& filename) {
	// first clear the existing settings
	settings_.clear();
	// create an input stream
	std::ifstream settingsfile(filename.c_str());
	// the string variable containing the next line
	std::string line;
	// while the file has not reached its end...
	while (std::getline(settingsfile, line)) {
		// ... check if the line is not a comment line ...
		if (line.c_str()[0]!='#') {
			// ... and find the '=' sign.
			size_t pos = line.find_first_of("=");
			std::string param, value;
			// get the first part (property/param) of the string
			param = line.substr(0,pos);  trimspaces(param);
			// get the last part (value) of the string
			value = line.substr(pos+1);  trimspaces(value);
			// store the param/value pair in the settings_ variable
			settings_[param]=value;
		}
	}
	// finally, close the input stream
	settingsfile.close();
	propagateSettings();
}

/*!
 * \brief Propagate the settings to ODE.
 */
void GPPhysicsSystem::propagateSettings() {
	std::map<std::string,std::string>::iterator sIt;

	dSurfaceParameters surface;
	surface.mode = 0;

	for (sIt=settings_.begin(); sIt!=settings_.end(); ++sIt) {
		if (sIt->first == "mode") {
			if (sIt->second.find("dContactMu2")!=sIt->second.npos) surface.mode = surface.mode | dContactMu2;
			if (sIt->second.find("dContactFDir1")!=sIt->second.npos) surface.mode = surface.mode | dContactFDir1;
			if (sIt->second.find("dContactBounce")!=sIt->second.npos) surface.mode = surface.mode | dContactBounce;
			if (sIt->second.find("dContactSoftERP")!=sIt->second.npos) surface.mode = surface.mode | dContactSoftERP;
			if (sIt->second.find("dContactSoftCFM")!=sIt->second.npos) surface.mode = surface.mode | dContactSoftCFM;
			if (sIt->second.find("dContactMotion1")!=sIt->second.npos) surface.mode = surface.mode | dContactMotion1;
			if (sIt->second.find("dContactMotion2")!=sIt->second.npos) surface.mode = surface.mode | dContactMotion2;
			if (sIt->second.find("dContactSlip1")!=sIt->second.npos) surface.mode = surface.mode | dContactSlip1;
			if (sIt->second.find("dContactSlip2")!=sIt->second.npos) surface.mode = surface.mode | dContactSlip2;
			if (sIt->second.find("dContactApprox0")!=sIt->second.npos) surface.mode = surface.mode | dContactApprox0;
			if (sIt->second.find("dContactApprox1_1")!=sIt->second.npos) surface.mode = surface.mode | dContactApprox1_1;
			if (sIt->second.find("dContactApprox1_2")!=sIt->second.npos) surface.mode = surface.mode | dContactApprox1_2;
			if (sIt->second.find("dContactApprox1")!=sIt->second.npos) surface.mode = surface.mode | dContactApprox1;
		}

		if (sIt->first == "mu") { if (sIt->second == "dInfinity") surface.mu = dInfinity; else surface.mu = (float)atof(sIt->second.c_str()); }
		if (sIt->first == "mu2") { if (sIt->second == "dInfinity") surface.mu2 = dInfinity; else surface.mu2 = (float)atof(sIt->second.c_str()); }
		if (sIt->first == "bounce") { if (sIt->second == "dInfinity") surface.bounce = dInfinity; else surface.bounce = (float)atof(sIt->second.c_str()); }
		if (sIt->first == "bounce_vel") { if (sIt->second == "dInfinity") surface.mu2 = dInfinity; else surface.bounce_vel = (float)atof(sIt->second.c_str()); }
		if (sIt->first == "soft_cfm") { if (sIt->second == "dInfinity") surface.soft_cfm = dInfinity; else surface.soft_cfm = (float)atof(sIt->second.c_str()); }
		if (sIt->first == "soft_erp") { if (sIt->second == "dInfinity") surface.soft_erp = dInfinity; else surface.soft_erp = (float)atof(sIt->second.c_str()); }
		if (sIt->first == "slip1") { if (sIt->second == "dInfinity") surface.slip1 = dInfinity; else surface.slip1 = (float)atof(sIt->second.c_str()); }
		if (sIt->first == "slip2") { if (sIt->second == "dInfinity") surface.slip2 = dInfinity; else surface.slip2 = (float)atof(sIt->second.c_str()); }
		if (sIt->first == "motion1") { if (sIt->second == "dInfinity") surface.motion1 = dInfinity; else surface.motion1 = (float)atof(sIt->second.c_str()); }
		if (sIt->first == "motion2") { if (sIt->second == "dInfinity") surface.motion2 = dInfinity; else surface.motion2 = (float)atof(sIt->second.c_str()); }
	}

	for (int i=0; i<MAX_CONTACTS; ++i) {
		contact_[i].surface = surface;
	}
}
//
///*!
// * \brief Apply additional forces to the object in ODE.
// */
void GPPhysicsSystem::applyAdditionalForces() {
	const dReal* v;
	const dReal* w;
	dReal wind[3] = {0.0,0,0};
	bool usespin = false;
	dReal nu     = 0.0f;
	dReal nuspin = 0.0f;
	dMass mass;
	//Drag coefficient
	float Cd = 1.0f;
	dReal rel_v[3], force[3], spinforce[3], torque[3];

//Air-density		
	float Ro =(float) std::atof(settings_["Ro"].c_str()); 
//sphere coefficient
	float sCd =(float) std::atof(settings_["sphere_Drag"].c_str());
//cube coefficient
	float cCd = (float)std::atof(settings_["cube_Drag"].c_str());

	if (settings_.find("use spin")!=settings_.end())
		usespin = (settings_["use spin"]=="1");

	if (settings_.find("wind")!=settings_.end()) {
		std::stringstream ss(settings_["wind"]);
		ss >> wind[0] >> wind[1] >> wind[2];
	}

	if (settings_.find("nu")!=settings_.end()) {
		nu = (float)atof(settings_["nu"].c_str());
	}

	if (settings_.find("nuspin")!=settings_.end()) {
		nuspin = (float)atof(settings_["nuspin"].c_str());
	}

	

	std::map<int, dBodyID>::iterator bodyIt = bodyIDs_.begin();
	//for all the bodies
	while (bodyIt != bodyIDs_.end()) {
		
		v = dBodyGetLinearVel(bodyIt->second);
		w = dBodyGetAngularVel(bodyIt->second);

		//wind influence
		rel_v[0] = -v[0]+wind[0]; rel_v[1] = -v[1]+wind[1]; rel_v[2] = -v[2]+wind[2]; 
		dReal velocity = dLENGTH(rel_v);
		

		//use spin
		if (usespin) {
			dCROSS(spinforce,=,rel_v,w);
		} else { spinforce[0]=0; spinforce[1]=0; spinforce[2]=0; }



		//Drag components
		float Fdx, Fdy, Fdz;
		//Unit vector indicating the direction of the velocity
		float unitx, unity, unitz;

		//True if this is a cube, false if this is a sphere
		if(dGeomGetClass(geometryIDs_[bodyIt->first])){
			
			//Calculate the drag using this formula: Fd = -0.5 * Ro * V^2 * Cd * A * v^
			dVector3 lengths;

			//find cube sides
			dGeomBoxGetLengths(geometryIDs_[bodyIt->first], lengths);
			//calculate Area
			float A = (float)lengths[0] * lengths[1];
			
			float length = pow(v[0], 2.f) + pow(v[1], 2.f) + pow(v[2], 2.f);
			unitx = v[0] / length;
			unity = v[1] / length;
			unitz = v[2] / length;

			Fdx = -0.5f * Ro * pow(v[0], 2.f) * cCd * A * unitx;
			Fdy = -0.5f * Ro * pow(v[1], 2.f) * cCd * A * unity;
			Fdz = -0.5f * Ro * pow(v[2], 2.f) * cCd * A * unitz;

			
		}else{
			//This is a sphere
			//Calculate the drag using this formula: Fd = -0.5 * Ro * V^2 * Cd * A * v^

			//find radius and then sphere_Area
			float r = (float)dGeomSphereGetRadius(geometryIDs_[bodyIt->first]);
			float A =(float) r * r * M_PI;
			
			//Unit vector indicating the direction of the velocity
			
			float length = pow(v[0], 2.f) + pow(v[1], 2.f) + pow(v[2], 2.f);
			unitx = v[0] / length;
			unity = v[1] / length;
			unitz = v[2] / length;

			Fdx =(float) -0.5f * Ro * pow(v[0], 2.f) * sCd * A * unitx;
			Fdy =(float) -0.5f * Ro * pow(v[1], 2.f) * sCd * A * unity;
			Fdz = (float)-0.5f * Ro * pow(v[2], 2.f) * sCd * A * unitz;

			
		}
		//end air-drag calculations
	
		//total force
		force[0] = nu * (velocity * rel_v[0]) + (nuspin * spinforce[0]) + Fdx;
		force[1] = nu * (velocity * rel_v[1]) + (nuspin * spinforce[1]) + Fdy;
		force[2] = nu * (velocity * rel_v[2]) + (nuspin * spinforce[2]) + Fdz; 

		dBodyAddForce(bodyIt->second, force[0], force[1], force[2]);

		dReal angularvelocity = dLENGTH(w);
		torque[0] = -nu * angularvelocity * w[0];
		torque[1] = -nu * angularvelocity * w[1];
		torque[2] = -nu * angularvelocity * w[2];

		dBodyAddTorque(bodyIt->second, torque[0], torque[1], torque[2]);


		//next body
		++bodyIt;
	}
}

/*!
 * \brief Apply additional forces to the object in ODE.
 */
//void GPPhysicsSystem::applyAdditionalForces() {
//	const dReal* v;
//	const dReal* w;
//	dReal wind[3] = {0.0,0,0};
//	bool usespin = false;
//	dReal nu     = 0.0f;
//	dReal nuspin = 0.0f;
//
//	if (settings_.find("use spin")!=settings_.end())
//		usespin = (settings_["use spin"]=="1");
//
//	if (settings_.find("wind")!=settings_.end()) {
//		std::stringstream ss(settings_["wind"]);
//		ss >> wind[0] >> wind[1] >> wind[2];
//	}
//
//	if (settings_.find("nu")!=settings_.end()) {
//		nu = (float)atof(settings_["nu"].c_str());
//	}
//
//	if (settings_.find("nuspin")!=settings_.end()) {
//		nuspin = (float)atof(settings_["nuspin"].c_str());
//	}
//
//	dReal rel_v[3], force[3], spinforce[3], torque[3];
//	std::map<int, dBodyID>::iterator bodyIt = bodyIDs_.begin();
//	while (bodyIt != bodyIDs_.end()) {
//
//		v = dBodyGetLinearVel(bodyIt->second);
//		w = dBodyGetAngularVel(bodyIt->second);
//		rel_v[0] = -v[0]+wind[0]; rel_v[1] = -v[1]+wind[1]; rel_v[2] = -v[2]+wind[2]; 
//		dReal velocity = dLENGTH(rel_v);
//
//		if (usespin) {
//			dCROSS(spinforce,=,rel_v,w);
//		} else { spinforce[0]=0; spinforce[1]=0; spinforce[2]=0; }
//
//
//		force[0] = nu * (velocity * rel_v[0]) + (nuspin * spinforce[0]);
//		force[1] = nu * (velocity * rel_v[1]) + (nuspin * spinforce[1]);
//		force[2] = nu * (velocity * rel_v[2]) + (nuspin * spinforce[2]);
//
//		dBodyAddForce(bodyIt->second, force[0], force[1], force[2]);
//
//		dReal angularvelocity = dLENGTH(w);
//		torque[0] = -nu * angularvelocity * w[0];
//		torque[1] = -nu * angularvelocity * w[1];
//		torque[2] = -nu * angularvelocity * w[2];
//
//		dBodyAddTorque(bodyIt->second, torque[0], torque[1], torque[2]);
//		++bodyIt;
//	}
//
//}




